package rov.data;


import java.util.List;
import java.util.ArrayList;
import java.util.LinkedList;
import java.util.Collections;
import java.io.InputStream;
import java.io.OutputStream;
import javax.swing.event.ChangeEvent;
import javax.swing.event.ChangeListener;

import java.io.InputStreamReader;
import java.io.BufferedReader;
import java.io.DataOutputStream;
import java.io.IOException;
import java.net.Socket;


// provides easy access to sensor data such as: gyro, accelerometer, etc
public class IMUData {
    private static final int PORT = 32001;

    private Socket mSock;
    private DataOutputStream mOut;
    private BufferedReader mIn;
    private double mX, mY, mZ, mDeltaT;
    private boolean mRunning;
    private Thread mUpdater;
    private List<ChangeListener> mListeners;
private char[] buf = new char[128];
private long last;
private boolean calib = true;
private double fixX = 34;
private double fixY = -1;
private double fixZ = -36;
private LinkedList<Integer> histX = new LinkedList<Integer>();
private LinkedList<Integer> histY = new LinkedList<Integer>();
private LinkedList<Integer> histZ = new LinkedList<Integer>();


    public IMUData(String serverName) {
        try {
            mSock = new Socket(serverName, PORT);
            mOut = new DataOutputStream(mSock.getOutputStream());
            mIn = new BufferedReader(new InputStreamReader(mSock.getInputStream()));
        } catch (IOException e) {
            System.err.println("error creating imu");
            e.printStackTrace();
        }

        mListeners = Collections.synchronizedList(
                         new ArrayList<ChangeListener>());

        mUpdater = new Thread() {
            public void run() {
                while (true) {
                    updateLoop();
                }
            }
        };

        mUpdater.start();

//mDeltaT = 0.01;
    }

    // Start listening for orientation data.
    public void start() {
        if (!mRunning) {
            mRunning = true;
        }
    }

    // Stop listening for orientation data.
    public void stop() {
        if (mRunning) {
            mRunning = false;
        }
    }

    // Returns true if listening for orientation data.
    public boolean isActive() {
        return mRunning;
    }

    // A listener's stateChanged method will be invoked when
    // orientation data is received.
    // Listeners' stateChanged methods should execute quickly.
    // Try not to do anything besides save the values from the getXYZ methods.
    public void addListener(ChangeListener l) {
        mListeners.add(l);
    }

    public double getX() {
        return mX;
    }

    public double getY() {
        return mY;
    }

    public double getZ() {
        return mZ;
    }

    public double getDeltaT() {
        return mDeltaT;
    }

    // Reads orientation data from the network.
    // Invokes listeners' stateChanged event handlers.
    private void updateLoop() {
        if (readData() && mRunning) {
            ChangeEvent e = new ChangeEvent(this);

            for (ChangeListener cl : mListeners) {
                cl.stateChanged(e);
            }
        }
    }

    private boolean readData() {
                    try {
//System.out.println("INPUT! " + (q++));

                        char c = 0;

                        while (c != 'x')
                            c = (char) mIn.read();

                        int i = 0;

                        while (c != '\n') {
                            buf[i] = c;
                            i++;
                            c = (char) mIn.read();
                        }

long now = System.currentTimeMillis();

                        String s = new String(buf, 0, i);

                        try {
                            int off1 = s.indexOf("=") + 1;
                            int off2 = s.indexOf(",", off1);
                            int off3 = s.indexOf("=", off2) + 1;
                            int off4 = s.indexOf(",", off3);
                            int off5 = s.indexOf("=", off4) + 1;
                            //int off6 = s.indexOf(",", off5);
                            int off6 = s.length();

//System.out.println("parse: " + s);
//System.out.println("strings: " + s.substring(2, off1).trim() +
//" | " + s.substring(off1+4, off2).trim() + " | " + s.substring(off2+4).trim());

if (!calib) {

                            mX = (Integer.parseInt(s.substring(off1, off2).trim()) + fixX) / 14.375;
                            mY = (Integer.parseInt(s.substring(off3, off4).trim()) + fixY) / 14.375;
                            mZ = (Integer.parseInt(s.substring(off5, off6).trim()) + fixZ) / 14.375;

mDeltaT = (now - last) / 1000.0;

if (mDeltaT > 1) mDeltaT = 0.03;

/*
                            mX = (Integer.parseInt(s.substring(off1, off2).trim()) + fixX);
                            mY = (Integer.parseInt(s.substring(off3, off4).trim()) + fixY);
                            mZ = (Integer.parseInt(s.substring(off5, off6).trim()) + fixZ);
*/
//System.out.println("o: " + mX + " -- " + mY + " -- " + mZ + " -- " + (now-last));

last = now;
                            //updateLoop();
return true;
} else {
                            histX.add(Integer.parseInt(s.substring(off1, off2).trim()));
                            histY.add(Integer.parseInt(s.substring(off3, off4).trim()));
                            histZ.add(Integer.parseInt(s.substring(off5, off6).trim()));

    if (histX.size() > 50 * 5) {
        calib = false;
        int sum = 0;
        for (Integer d : histX)
            sum += d;
        fixX = -sum / (double) histX.size();
        histX.clear();
//        histX = null;

        sum = 0;
        for (Integer d : histY)
            sum += d;
        fixY = -sum / (double) histY.size();
        histY.clear();
//        histY = null;

        sum = 0;
        for (Integer d : histZ)
            sum += d;
        fixZ = -sum / (double) histZ.size();
        histZ.clear();
//        histZ = null;

System.out.println("fixes: " + fixX + " " + fixY + " " + fixZ);
    }

}
                        } catch (NumberFormatException ex) {
                            //System.out.println(ex + " parsing IMU output");
//ex.printStackTrace();
//System.out.println("error: \"" + (new String(buf)) + "\"");
System.out.println("err");
                        } catch (StringIndexOutOfBoundsException ex) {
System.out.println("err2 \"" + s + "\"");
                        }
                    } catch (IOException e) {
                       	System.out.println("Error reading IMU output: " + e);
                    }
return false;
    }
}
